/*
 * steering_control.c
 *
 *  Created on: 2012.12.20.
 *      Author: Peter
 */
#include "stm32f4xx.h"
#include "steering.h"
#include "steering_control.h"
#include "LineTracker.h"

float str_prev_err = 0;
float str_int_err = 0;
#define SRT_INT_MAX 500

void Steering_Control(float ref, float input, float pp, float pi, float pd)
{
	float diff;
	float err;
	int16_t angle;

	/* P */
	err =  ref - input;
	/* I */

	if(str_int_err > 50)
	{
		if(str_int_err*err >= 0)
			str_int_err += err * (SRT_INT_MAX-str_int_err)/50;
		else
			str_int_err += err * str_int_err / 50;
	}
	else str_int_err += err;

	if(str_int_err > SRT_INT_MAX) str_int_err = SRT_INT_MAX;
	if(str_int_err < -SRT_INT_MAX) str_int_err = -SRT_INT_MAX;

	/* D */
	diff = err - str_prev_err;
	str_prev_err = err;

	angle = (int16_t)(err * pp + str_int_err * pi + diff * pd);
	//angle = (int16_t)(err * 50 + str_int_err * 1 + diff * 500);

	if(angle>1050) angle = 1050;
	else if(angle < -1050)angle = -1050;

	SetSteeringPWM(angle);
}
